/*
 * Copyright 2021 <copyright holder> <email>
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <cstdio>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

#include "rotation.h"
#include "random.h"
#include "common.h"

typedef Eigen::Map<Eigen::VectorXd> VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;

template<typename T>
void FscanForDie(FILE *fptr, const char *format, T *value){
    int num_scanned=fscanf(fptr,format, value);
    if(num_scanned!=1)
        std::cerr<<"Invalid UW data file. ";
}

void PerturbPoint3(const double sigma, double *point){
    for(int i=0;i<3;i++)
        point[i]+=RandNormal()*sigma;
}//添加噪音？？？

double Median(std::vector<double> *data){
    int n=data->size();
    std::vector<double>::iterator mid_point=data->begin()+n/2;
    std::nth_element(data->begin(),mid_point,data->end()); //部分排序使第n大元素处于第n位置（从0开始,其位置是下标为 n的元素），并且比这个元素小的元素都排在这个元素之前，比这个元素大的元素都排在这个元素之后
    return *mid_point;
}

BALProblem::BALProblem::BALProblem(const std::string& filename, bool use_quaternions)
{
    FILE *fptr=fopen(filename.c_str(),"r");
    if(fptr==NULL){
        std::cerr<<"Error: unable to open file"<<filename;
        return ;
    }
    
    //this will die horribly on invalid files. Them's the breaks.
    FscanForDie(fptr,"%d",&num_camreas_);
    FscanForDie(fptr,"%d",&num_points_);
    FscanForDie(fptr,"%d",&num_observations_);
    
    std::cout<<"Header:"<<num_camreas_
            <<" "<<num_points_
            <<" "<<num_observations_;
            
    point_index_=new int[num_points_];
    camera_index_=new int[num_camreas_];
    observations_index_=new double[num_observations_*2];
    
    num_parameters_=9*num_cameras_+3*num_points_;
    paramaters_=new double[num_parameters_];
    
    for(int i=0;i<num_observations_;++i){
        FscanForDie(fptr, "%d",camera_index_+i);
        FscanForDie(fptr, "%d",point_index_+i);
        for(int j=0;j<2;++j){
            FscanForDie(fptr,"%lf",observations_+2*i+j);
        }
    }
    
    fclose(fptr);
    
    use_quaternions_=use_quaternions;
    if(use_quaternions){
        //switch the angle_axis rotations to use_quaternions.
        num_parameters_=10*num_camreas_+3*num_points_;  //why is 10 *num_cameras_
        double *quaternion_parameters=new double[num_parameters_];
        double *original_cursor=parameters_;
        double *quaternion_cursor=quaternion_parameters;
        for(int i=0;i<num_camreas_;++i){
            AngleAxisToQuaternion(original_cursor,quaternion_cursor);
            quaternion_cursor+=4;
            original_cursor+=3;
            for(int j=4;j<10;++j){
                *quaternion_cursor+=*original_cursor++;
            }
        }
        
         // Copy the rest of the points.
        for(int i=0;i<3*num_points_;++i){
            *quaternion_cursor++=*original_cursor++;
        }
        // Swap in the quaternion parameters.
        delete[] parameters_;
        parameters_=quaternion_parameters;
    }
}
    
void BALProblem::WriteToFile(const std::string& filename) const
{
    FILE *fptr=fopen(filename.c_str(),"w" );
    if(fptr==NULL){
        std::cerr<<"Error:unable to open file"<<filename;
        return;
    }
    fprintf(fptr, "%d %d %d %d\n", num_cameras_,num_parameters_,num_points_,num_observations_);
    
    for(int i=0;i<num_observations_;++i){
        fprintf(fptr,"%d,%d",camera_index_[i],point_index_[i]);
        for(int j=0;j<2;++j){
            fprintf(fptr,"%g",observations_[2*i+j]);
        }
        fprintf(fptr,"\n");
    }
    
    for(int i=0;i<num_camreas();++i){
        double anglexis[9];
        if(use_quaternions_){
            //output in angle_axis format
            QuaternionToAngleAxis(parameters_+10*i,angleaxis);
            memcpy(anglexis+3,paramaters_+10*i+4,6*sizeof(double));
        }else{
            memcpy(angleaxis,parameters_+9*i,9*sizeof(double));
        }
        for(int j=0;j<9;++j){
            fprintf(fptr,"%.16g\n",angleaxis[j]);
        }
    }
    
    const double *points=parameters_+camera_block_size()*num_camreas_;
    for(int i=0;i<num_points_;i++){
        const double *point=points+i*point_block_size();
        for(int j=0;j<point_block_size();++j){
            fprintf(fptr,"%16g/n",point[j]);
        }
    }
    fclose(fptr);
}


//write the problem to a PLY file for inspection in meshlab or CloudCompare
void BALProblem::WriteToPLYFile(const std::string& filename) const
{
    std::ofstream of(filename.c_str());
    of<<"ply"
      <<'\n'<<"format ascii 1.o"
      <<'\n'<<"element vertex "<<num_camreas_+num_points_
      <<'\n'<<" property float x"
      <<'\n'<<" property float y"
      <<'\n'<<" property float z"
      <<'\n'<<" property uchar red"
      <<'\n'<<" property uchar green"
      <<'\n'<<" property uchar blue"
      <<'\n'<<" end_header"<<std::endl;
      
      //export extrinsic data (i,e. camera centers) as green points.
      double angle_axis[3];
      double center[3];
      for(int i=0;i<num_cameras();++i){
          const double *camera=cameras()+camera_block_size()*i;
          CameraToAngelAndCenter(camera,angle_axis,center);
          of<<center[0]<<' '<<center[1]<<' '<<center[2]<<" 0 255 0 "<<'\n';
      }
      
      // Export the structure (i.e. 3D Points) as white points.
      const double *points=parameters_+camera_block_size()*num_cameras_;
      for(int i=0;i<num_points();++i){
          const double *point=points+i*point_block_size();
          for(int j=0;j<point_block_size();++j){
              of<<point[j]<<' ';
          }
          of<<" 255 255 255\n";
      }
      of.close();
}
    
    
    
    
    
    
    
    
            
    
    
}

    
    






















void BALProblem::Perturb(const double rotation_sigma, const double translation_sigma, const double point_sigma)
{
    assert(point_sigma>=0.0); //debug的断言，不满足时程序执行中断
    assert(rotation_sigma>=0.0);
    assert(translation_sigma>=0.0);
    
    double *points=mutenble_points();
    if(point_sigma>0){
        for(int i=0; i<num_points_;++i){
            PerturbPoint3(point_sigma,points+3*i);
        }
    }
    
    for (int i=0;i<num_cameras_;++i){
        double *camera=mutable_cameras()+camera_block_size()*i;
        double angle_axis[3];
        double center[3];
        
        
        
        
        
        
        
